#include "ARM.h"

#include "FreeRTOS.h"
#include "task.h"
#include "main.h"
#include "cmsis_os.h"

#include "mainc.h"
#include "controller.h"
#include "DJI_motor.h"


#include "usart.h"
#include "chassis.h"
PIDInstance Armo1[2];
PIDInstance Armo2[2];
PIDInstance Armo3[2];
PIDInstance Armo4[2];
PIDInstance Armo5[2];
float Aout1[2], Aout2[2], Aout3[2], Aout4[2], Aout5[2];
PID_Init_Config_s Armo1_pid[2] = {
    {.Kp = 40,
     .Ki = 15,
     .Kd = 0.1,
     .MaxOut = 16000,
     .Improve = PID_Integral_Limit,
     .IntegralLimit = 50},
    {.Kp = 8,
     .Ki = 0.01,
     .Kd = 0.01,
     .MaxOut = 10000,
     .Improve = PID_Integral_Limit,
     .IntegralLimit = 1000}};
PID_Init_Config_s Armo2_pid[2] = {
    {.Kp = 40,
     .Ki = 15,
     .Kd = 0.1,
     .MaxOut = 15000,
     .Improve = PID_Integral_Limit,
     .IntegralLimit = 50},
    {.Kp = 8,
     .Ki = 0.01,
     .Kd = 0.01,
     .MaxOut = 10000,
     .Improve = PID_Integral_Limit,
     .IntegralLimit = 1000}};
PID_Init_Config_s Armo3_pid[2] = {
    {.Kp = 40,
     .Ki = 15,
     .Kd = 0.1,
     .MaxOut = 15000,
     .Improve = PID_Integral_Limit,
     .IntegralLimit = 50},
    {.Kp = 8,
     .Ki = 0.01,
     .Kd = 0.01,
     .MaxOut = 12000,
     .Improve = PID_Integral_Limit,
     .IntegralLimit = 1000}};
PID_Init_Config_s Armo4_pid[2] = {
    {.Kp = 40,
     .Ki = 15,
     .Kd = 0.1,
     .MaxOut = 16000,
     .Improve = PID_Integral_Limit,
     .IntegralLimit = 50},
    {.Kp = 8,
     .Ki = 0.01,
     .Kd = 0.01,
     .MaxOut = 15000,
     .Improve = PID_Integral_Limit,
     .IntegralLimit = 1000}};
PID_Init_Config_s Armo5_pid[2] = {
    {.Kp = 40,
     .Ki = 15,
     .Kd = 0.1,
     .MaxOut = 16000,
     .Improve = PID_Integral_Limit,
     .IntegralLimit = 50},
    {.Kp = 8,
     .Ki = 0.01,
     .Kd = 0.01,
     .MaxOut = 10000,
     .Improve = PID_Integral_Limit,
     .IntegralLimit = 1000}};

int32_t first_pos = 1961;
int32_t second_pos = 37248;
int32_t endd_pos = 47629;
		 
int32_t first_pos5 = 38413;
int32_t second_pos5 = 71894;
int32_t endd_pos5 = 54040;
		 
uint16_t first_tim = 0;
uint8_t Down[4]={0,0,0,0}; 
uint8_t RxData[8]={0,0};
//uint8_t pose_in[5]={0};
uint8_t i=0;
extern uint8_t or_sencod[5];

void AMoto_Zero(void); // 角度归零
void PID_clc(void);
void ArmPIDInit(void);
// 1 3号为-
void AMotor(void *argument)
{
    vTaskDelay(1000);
    uint32_t current_time; //-3450000
    ArmPIDInit();
    AMoto_Zero();
    vTaskDelay(500);
	
		
//	HAL_UART_Receive_DMA(&huart10,RxData,10); 

	
    for (;;)
    {
        current_time = xTaskGetTickCount();
        if (CMode == 2)
        {
					
//            Arm_Motor[0].target_angle = -first_pos;
//            Arm_Motor[1].target_angle = first_pos;
//            Arm_Motor[2].target_angle = -first_pos;
//            Arm_Motor[3].target_angle = first_pos;
//            Arm_Motor[4].target_angle = -first_pos5;
					
					
					if((RxData[1]&0x01)==0x01)
						Arm_Motor[0].target_angle = -first_pos;
					else
						Arm_Motor[0].target_angle = -second_pos;
					if((RxData[1]&0x02)==0x02)
						Arm_Motor[1].target_angle =first_pos;
					else
						Arm_Motor[1].target_angle = second_pos;
					
					if((RxData[1]&0x04)==0x04)
						Arm_Motor[2].target_angle = -first_pos;
					else
						Arm_Motor[2].target_angle = -second_pos;
					
					if((RxData[1]&0x08)==0x08)
						Arm_Motor[3].target_angle = first_pos;
					else
						Arm_Motor[3].target_angle = second_pos;
					
					if((RxData[1]&0x10)==0x10)
						Arm_Motor[4].target_angle = -second_pos5;
					else
						Arm_Motor[4].target_angle = -first_pos5;
					
					/*
					for(i=0;i<5;i++)
					{
						if((RxData[i]&(0x01<<i))==(0x01<<i))
						{
							if(i==4)
								Arm_Motor[i].target_angle = -second_pos5;
							else
								Arm_Motor[i].target_angle = first_pos;
						}
						else
						{
							if(i==4)
								Arm_Motor[i].target_angle = -first_pos5;
							else
								Arm_Motor[i].target_angle = second_pos;
						}
					}
					*/
            int32_t err1, err2, err3, err4, err5;
            err1 = Arm_Motor[0].target_angle - Arm_Motor[0].totall_angle;
            err2 = Arm_Motor[1].target_angle - Arm_Motor[1].totall_angle;
						err3 = Arm_Motor[2].target_angle - Arm_Motor[2].totall_angle;
						err4 = Arm_Motor[3].target_angle - Arm_Motor[3].totall_angle;
						err5 = Arm_Motor[4].target_angle - Arm_Motor[4].totall_angle;
						
						if(err1<18000&&err1>-18000&&(RxData[1]&0x1)!=0x1)
							Down[0] = 1;
						if(err2<18000&&err2>-18000&&(RxData[1]&0x2)!=0x2)
							Down[1] = 1;
						if(err3<18000&&err3>-18000&&(RxData[1]&0x4)!=0x4)
							Down[2] = 1;
						if(err4<18000&&err4>-18000&&(RxData[1]&0x8)!=0x8)
							Down[3] = 1;
						
            if (err2 < 10 && err1 < 10&&err3 < 10 &&err4 < 10  &&err1 > -10 && err2 > -10&&err3 > -10&&err4 > -10&&err5<10 &&err5>-10)
            {
                first_tim++;
                if (first_tim > 20)
                {
                    CMode++;
                    first_tim = 0;
                }
            }
        }
				
//        else if (CMode == 4)
//        {
//            for (uint8_t i = 0; i < 5; i++)
//            {
//                if (or_sencod[i] == 1)
//                    if (i == 0 | i == 2) // 方向相反
//                        Arm_Motor[i].target_angle = -second_pos;
//                    else
//                        Arm_Motor[i].target_angle = second_pos;
//            }
//            first_tim++;
//            if (first_tim > 250 * 6)
//            {
//                CMode++;
//                first_tim = 0;
//            }
//        }
        if (CMode == 4)	//7
        {
            Arm_Motor[1].target_angle = endd_pos+400;
            Arm_Motor[2].target_angle = -endd_pos-800;
						Arm_Motor[0].target_angle = -endd_pos;
            Arm_Motor[3].target_angle = endd_pos;
						Arm_Motor[4].target_angle = -endd_pos5;
            int32_t err1, err2, err3, err4, err5;
            err1 = Arm_Motor[1].target_angle - Arm_Motor[1].totall_angle;
            err2 = Arm_Motor[2].target_angle - Arm_Motor[2].totall_angle;
						err5 = Arm_Motor[4].target_angle - Arm_Motor[4].totall_angle;
            err3 = Arm_Motor[0].target_angle - Arm_Motor[0].totall_angle;
            err4 = Arm_Motor[3].target_angle - Arm_Motor[3].totall_angle;
            if (err2 < 20 && err1 < 20 && err3<20 &&	err4<20 && err1 > -20 && err2 > -20 && err3>-20 &&err4>-20	&&err5<20 &&err5>-20)
            {
                first_tim++;
                if (first_tim > 20)
                {	
									
									if(chassis_flag1==1)
									{
										CMode++;
                    first_tim = 0;
									}
                    
                }
            }
        }
        else if (CMode == 6)	//9
        {
            Arm_Motor[1].target_angle = 43000;
            Arm_Motor[2].target_angle = -43000;
						Arm_Motor[4].target_angle = -43000;
            int32_t err1, err2;
            err1 = Arm_Motor[0].target_angle - Arm_Motor[0].totall_angle;
            err2 = Arm_Motor[3].target_angle - Arm_Motor[3].totall_angle;
            if (err2 < 10 && err1 < 10 && err1 > -10 && err2 > -10)
            {
                first_tim++;
                if (first_tim > 20)
                {
                    CMode++;
                    first_tim = 0;
                }
            }
        }
        else if (CMode == 7)	//11
        {
            Arm_Motor[0].target_angle = -endd_pos;
            Arm_Motor[3].target_angle = endd_pos;
            int32_t err1, err2;
            err1 = Arm_Motor[0].target_angle - Arm_Motor[0].totall_angle;
            err2 = Arm_Motor[3].target_angle - Arm_Motor[3].totall_angle;
            if (err2 < 10 && err1 < 10 && err1 > -10 && err2 > -10)
            {
                first_tim++;
                if (first_tim > 30)
                {
									if(chassis_flag2==1)
									{
										CMode++;
                    first_tim = 0;
									}
                    
                }
            }
        }
        else if (CMode == 9)	//13
        {
            Arm_Motor[0].target_angle = -43000;
            Arm_Motor[3].target_angle = 43000;
            int32_t err1, err2;
            err1 = Arm_Motor[0].target_angle - Arm_Motor[0].totall_angle;
            err2 = Arm_Motor[3].target_angle - Arm_Motor[3].totall_angle;
            if (err2 < 10 && err1 < 10 && err1 > -10 && err2 > -10)
            {
                first_tim++;
                if (first_tim > 3000)
                {
                    //CMode++;
                    first_tim = 0;
                }
            }
        }
				
				else if (CMode == 10)	//13
        {
            Arm_Motor[1].target_angle = 0;
            Arm_Motor[2].target_angle = 0;
						Arm_Motor[0].target_angle = 0;
            Arm_Motor[3].target_angle = 0;
						Arm_Motor[4].target_angle = 0;
        }

        if (CMode == 0)
        {
            Arm_Motor[0].tx_data = 0;
            Arm_Motor[1].tx_data = 0;
            Arm_Motor[2].tx_data = 0;
            Arm_Motor[3].tx_data = 0;
            Arm_Motor[4].tx_data = 0;
        }
        else
        {
            PID_clc();
            Arm_Motor[0].tx_data = Aout1[1];
            Arm_Motor[1].tx_data = Aout2[1];
            Arm_Motor[2].tx_data = Aout3[1];
            Arm_Motor[3].tx_data = Aout4[1];
            Arm_Motor[4].tx_data = Aout5[1];
        }
        Arm_Send();
        vTaskDelayUntil(&current_time, 5); // 绝对延时
    }
}

void AMoto_Zero(void) // 角度归零
{
    for (uint8_t i = 0; i < 5; i++)
    {
        Arm_Motor[i].offset_angle = Arm_Motor[i].angle;
        Arm_Motor[i].round = 0;
        Arm_Motor[i].target_angle = 0;
    }
}

void PID_clc(void)
{
    Aout1[0] = PIDCalculate(&Armo1[0], Arm_Motor[0].totall_angle, Arm_Motor[0].target_angle); // 位置环
    Aout1[1] = PIDCalculate(&Armo1[1], Arm_Motor[0].rpm, Aout1[0]);

    Aout2[0] = PIDCalculate(&Armo2[0], Arm_Motor[1].totall_angle, Arm_Motor[1].target_angle); // 位置环
    Aout2[1] = PIDCalculate(&Armo2[1], Arm_Motor[1].rpm, Aout2[0]);

    Aout3[0] = PIDCalculate(&Armo3[0], Arm_Motor[2].totall_angle, Arm_Motor[2].target_angle); // 位置环
    Aout3[1] = PIDCalculate(&Armo3[1], Arm_Motor[2].rpm, Aout3[0]);

    Aout4[0] = PIDCalculate(&Armo4[0], Arm_Motor[3].totall_angle, Arm_Motor[3].target_angle); // 位置环
    Aout4[1] = PIDCalculate(&Armo4[1], Arm_Motor[3].rpm, Aout4[0]);

    Aout5[0] = PIDCalculate(&Armo5[0], Arm_Motor[4].totall_angle, Arm_Motor[4].target_angle); // 位置环
    Aout5[1] = PIDCalculate(&Armo5[1], Arm_Motor[4].rpm, Aout5[0]);                           // 速度环
}

void ArmPIDInit(void)
{
    PIDInit(&Armo1[0], &Armo1_pid[0]);
    PIDInit(&Armo1[1], &Armo1_pid[1]);

    PIDInit(&Armo2[0], &Armo2_pid[0]);
    PIDInit(&Armo2[1], &Armo2_pid[1]);

    PIDInit(&Armo3[0], &Armo3_pid[0]);
    PIDInit(&Armo3[1], &Armo3_pid[1]);

    PIDInit(&Armo4[0], &Armo4_pid[0]);
    PIDInit(&Armo4[1], &Armo4_pid[1]);

    PIDInit(&Armo5[0], &Armo5_pid[0]);
    PIDInit(&Armo5[1], &Armo5_pid[1]);
}

uint16_t size;

//void HAL_UARTEx_RxEventCallback(UART_HandleTypeDef *huart, uint16_t Size)
//{
//	size = Size;
//	printf("%d\n",size);

//}

void HAL_UART_IdleCpltCallback(UART_HandleTypeDef *huart)
{
//	printf("ss\n");
}